/****************************************************************************
 * Project developed as a simple Blink the LED's to 
 * demonstrate basic CodeWarrior functionality and capability.
 * Borrowed from Freedom Example Projects: Blinky
 * 
 * Date: 11 Sept 2012
 * Author: M. Ruthenbeck
 * Revision Level 1.0
 */

#include "derivative.h" /* include peripheral declarations */
#include "Mytypes.h"
#include "PWM_Config.h"
#include "Port_Config.h"
#include "InputCapture.h"
#include "GPIO.h"
#include "ADC.h"

#if FreescaleCup
#include "PIT.h"
#else 
#include "IC_Headers/IC_PIT.h"
#endif

#include "ServoMotor.h"
#include "IC_Headers/IC_PortConfig.h"
#include "IC_Headers/IC_PwmConfig.h"
#include "IC_Headers/IC_ADCConfig.h"
#include "mcl\CrystalClock.h"
#include "mcl\ARM_SysTick.h"
#include "gsc_scheduler\gsc_sch_core\gsc_sch_core_intf.h"

volatile unsigned int sys_tick_counter = 0;



void WatchDog_cfg(void);
void Enable_Irq(uint8);
/********************************************************************/
int main (void)
{
	
	//int i=0;
	Frec_Flag = 0;
	
	/* Disable Watch Dog */
	WatchDog_cfg();
	/* initialize clock system for 48 MHz */
	InitClock(); 
	/* Configure the timer and the interrupt to be used to generate the tick of the scheduler */
	InitSysTick();
	/* Interrupt Enable */
	Enable_Irq(INT_TPM2);
	
#if FreescaleCup
	/* Configure the ports as input or output*/
	vdfn_Port_Config();
	/* Configure the TPM0 as a PWM for the motors*/
	vdfn_PwmInit();
	/* Configure the TPM2 as Input Capture*/
	vdfn_InputCapture_Init();
	//GPIO INIT
	vdfn_GPIO_INIT();
	//Servo INIT
	vdfn_ServoInit();
	//ADC Init
	vdfn_ADC_Init();
	//PIT Init
	vdfn_PIT_Init();
	Enable_Irq(INT_PIT);
	//SPI Init (Camera Clock) 
	//vdfn_Camera_Init();
#else
	/*Ports initialization*/
	vdfn_IC_PortConfig();
	/*PWM initialization*/
	vdfn_IC_PwmInit();
	/*SERVO Initialization*/
	vdfn_IC_ServoInit();
	//ADC_POT_INIT
	vdfn_IC_ADCConfig();
	//GPIO INIT
	vdfn_GPIO_INIT();
	//PIT INIT
	vdfn_IC_PIT_Init();
	Enable_Irq(INT_PIT);
#endif	
	

	/* Scheduler Initialization and tasks initialization  */
	gsc_sch_core_Init();
	/* Execute Scheduler */
	gsc_sch_core_exec();

	return 0;
}

 
 /*
  * Init watch dog
  */
 void WatchDog_cfg(void)
 {
	 /* Disable watch dog */
	 SIM_COPC |= SIM_COPC_COPT(0);
 }
 
 void SysTick_Handler(void)
 {
 	sys_tick_counter++;
 	gsc_sch_core_tick_isr();
 }

 void Enable_Irq(uint8 irq){
	 
	 irq = irq - 16;
	 
	 if(irq > 32){
		 
	 }
	 else{
	 NVIC_ICPR |= (1 << irq%32);
	 NVIC_ISER |= (1 << irq%32);
	 }
 }
